#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Description:       :
@Date     :2021/04/11 19:44:30
@Author      :chenqi
@version      :1.0
'''
from typing import Tuple
import numpy as np
import math
from pyproj import Geod
import numpy as np

# def transform2(lon, lat):
#     x = lon * 20037508.342789 / 180
#     y = math.log(math.tan((90 + lat) * math.pi / 360)) / (math.pi / 180)
#     y = y * 20037508.34789 / 180
#     x = x - 13933099.894061854
#     y = y - 5439551.114445192
#     theta = 17.0 / 180 * math.pi
#     x_ = x * math.cos(theta) + y * math.sin(theta)
#     y_ = y * math.cos(theta) - x * math.sin(theta)
#     temp = x_
#     x_ = y_
#     y_ = temp
#     x_ = 97.55 / 149 * x_
#     y_ = - 15.0 / 19.8 * y_
#     return (x_, y_)


def transform(lon, lat, angle):##我没进行太严密的测试，可能会出问题。
    geod = Geod(ellps = "WGS84") #按wgs84的格式进行转换
    if angle < 180:
        angle = - angle
    else:
        angle = 360 - angle
    lon, lat, back_az = geod.fwd(lon, lat, angle, 0)
    #start_lon = 125.1551018#起始经纬度坐标 这个得录完包以后告诉你
    #start_lat = 43.8334708
    start_lon = 125.15510914198666
    start_lat = 43.833471756275614
    theta =  -71.4 / 180 * np.pi  #经纬度坐标系与图坐标系的转换角，这个也得录完包之后告诉你
    forward_az, back_az, dist = geod.inv(start_lon, start_lat, lon, lat)#forward x轴偏转角， dist距离
    #print(dist)
    world_x = dist * np.cos(forward_az * np.pi / 180)
    world_y = dist * np.sin(forward_az * np.pi / 180)  #转成世界坐标系下的坐标
    #print(world_x,world_y,np.sin(theta), np.cos(theta))
    #转成图中坐标，这里面XY是反的
    x_in_map = world_x * np.cos(theta) - world_y * np.sin(theta)
    y_in_map = world_y * np.cos(theta) + world_x * np.sin(theta)

    angle = angle - 71.4
    return (x_in_map, y_in_map, angle)


def calc_accelerate(lon: float, lat: float):
    return math.sqrt(math.pow(lon,2) + math.pow(lat, 2))

# 地图坐标系的初始点，根据此点可以计算出方位角和距离，从而得到地图坐标系的坐标
# 初始点的经度
ori_lon = 125.1550768
# 初始点的纬度
ori_lat = 43.8334643


def transform_old(lon, lat):
    """
            input——lon为经度，lat为纬度
            output——返回地图坐标系下的x，y坐标
    """
    # geod.inv第一个输出值为角度，第三个为弧度
    # math.cos,math.sin输入是弧度，因此需要先将角度转弧度
    geod = Geod(ellps="WGS84")
    forward_amz, backward_amz, distance = geod.inv(ori_lon, ori_lat, lon, lat)
    # print(forward_amz,backward_amz,distance)
    return distance*math.cos(math.radians(forward_amz)), -1*distance*math.sin(math.radians(forward_amz))


def do_transform(p: np.array, r: np.array, t: np.array):
    return np.dot(r, p) + t


def rad(angle: float) -> float:
    return angle*math.pi/180


def calc_index(center_x: float, center_y: float, min_x: float, min_y: float, row: int) -> int:
    from math import ceil
    k = ceil((center_y - min_y) / 4.43)
    b = ceil((center_x - min_x) / 1.59)
    return k*row + b

# print(transform(125.1627880, 43.8333853))
# print(transform(125.1629514, 43.8334184))
